﻿using CGXi_Sdk;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using static SkiaSharp.HarfBuzz.SKShaper;

namespace JES
{
    
    public class RobotHelper
    {
        public static int robotHandle = -1;

        //姿态
        //public static double[] basePose = new double[6] { -58.88, -18.63, -62.04, -95.78, 37.25, 213.61 };
        public static double[] basePose = new double[6] { -74.54, 15.22, -87.28, -18.89, 91.37, 6.28 };
        //public static double[] basePose = new double[6] { -56.28, -7.33, -68.70, -96.44, 24.68, 213.17 };
        //点位
        public static double[] basepoint = new double[6] { -215.74, -455.97, 361.95, 177.03, -28.85, -1.26 };

        //当前机械臂运行的相对位置
        public static double[] robotPoint = new double[6] { 0,0, 0, 0, 0, 0 };



        public RobotHelper()
        {
            //连接机械人
            CRresult result = CS.cr_create_robot(ref robotHandle, "192.168.1.200", 2323, "123");
            if (result != CRresult.sucess)
                Console.WriteLine("连接机械人失败");
        }


        /// <summary>
        /// 回到原点
        /// </summary>
        public void ReturnOrigin()
        {
            PointControlParaSimple pointControlParaSimple = new PointControlParaSimple();
            PointControlPara pointControlPara = new PointControlPara();

            bool isBlock = true;
            pointControlParaSimple.speed = new double[6] {25, 25, 25, 25, 25, 25 };
            pointControlParaSimple.acc = new double[6] {25, 25, 25, 25, 25, 25 };
            pointControlParaSimple.pose = basepoint;
            //在MoveJ中无影响
            pointControlParaSimple.jointpos = basePose;
            //目标点示例数据
            pointControlParaSimple.tcpOffset = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlParaSimple.coordinatePose = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlParaSimple.coordinateType = CoordinateType.jointCoordinate;
            pointControlParaSimple.pointTransType = PointTransType.pointTransStop;
            pointControlParaSimple.motiontriggerMode = MotiontriggerMode.MovetriggerbyOnlyRpc;
            CS.cr_move_pointControlPara_transfer(robotHandle, pointControlParaSimple, ref pointControlPara);
            CRresult result = CS.cr_move_joint(robotHandle, pointControlPara, isBlock);
        }


        /// <summary>
        ///  指定关节运动指定角度 或者 指定关节运动到指定角度
        /// </summary>
        /// <param name="index">要运动的轴 从0开始数</param>
        /// <param name="count">运动的角度</param>
        public void MovePos(int index,double  count, double speed = 5,bool isTo=false)
        {
            PointControlParaSimple pointControlParaSimple = new PointControlParaSimple();
            PointControlPara pointControlPara = new PointControlPara();

            bool isBlock = true;
            pointControlParaSimple.speed = new double[6] { speed, speed, speed, speed, speed, speed };
            pointControlParaSimple.acc = new double[6] { speed, speed, speed, speed, speed, speed };
            pointControlParaSimple.pose = basepoint;
            //获取当前关节姿态
            var pos = api_demo_cr_get_jointActualPos();
            //改变当前姿态
            if(isTo)
            {
                pos[index] =  count;
            }
            else
            {
                pos[index] = pos[index] + count;
            }
         
            //在MoveJ中无影响
            pointControlParaSimple.jointpos = pos;
            //目标点示例数据
            pointControlParaSimple.tcpOffset = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlParaSimple.coordinatePose = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlParaSimple.coordinateType = CoordinateType.jointCoordinate;
            pointControlParaSimple.pointTransType = PointTransType.pointTransStop;
            pointControlParaSimple.motiontriggerMode = MotiontriggerMode.MovetriggerbyOnlyRpc;
            CS.cr_move_pointControlPara_transfer(robotHandle, pointControlParaSimple, ref pointControlPara);
            CRresult result = CS.cr_move_joint(robotHandle, pointControlPara, isBlock);
        }


        /// <summary>
        ///  关节运动指定角度
        /// </summary>
   
        /// <param name="pos">姿态角度</param>
        public void MoveToPos(double [] pos,double speed = 5)
        {
            PointControlParaSimple pointControlParaSimple = new PointControlParaSimple();
            PointControlPara pointControlPara = new PointControlPara();

            bool isBlock = true;
            pointControlParaSimple.speed = new double[6] { speed, speed, speed, speed, speed, speed };
            pointControlParaSimple.acc = new double[6] { speed, speed, speed, speed, speed, speed };
            pointControlParaSimple.pose = basepoint;
   
            //在MoveJ中无影响
            pointControlParaSimple.jointpos = pos;
            //目标点示例数据
            pointControlParaSimple.tcpOffset = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlParaSimple.coordinatePose = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlParaSimple.coordinateType = CoordinateType.jointCoordinate;
            pointControlParaSimple.pointTransType = PointTransType.pointTransStop;
            pointControlParaSimple.motiontriggerMode = MotiontriggerMode.MovetriggerbyOnlyRpc;
            CS.cr_move_pointControlPara_transfer(robotHandle, pointControlParaSimple, ref pointControlPara);
            CRresult result = CS.cr_move_joint(robotHandle, pointControlPara, isBlock);
        }


        /// <summary>
        /// 移动X轴到指定点位
        /// </summary>
        /// <param name="number"></param>
        public void Xchange(double number)
        {
            //获取当前位置
            double[] pose = api_demo_cr_get_tcpActualPose();
            pose[0] =  number;

            PointControlPara pointControlPara = new PointControlPara();
            PointControlParaSimple pointControlParaSimple = new PointControlParaSimple();

            bool isBlock = true;
            pointControlParaSimple.speed = new double[6] {5, 5, 5, 5, 5, 5 };
            pointControlParaSimple.acc = new double[6] {5, 5, 5, 5, 5, 5 };
        
            pointControlParaSimple.jointpos = basePose;
            //起始点示例数据
            pointControlParaSimple.pose = pose;
            //目标点示例数据
            pointControlParaSimple.tcpOffset = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlParaSimple.coordinatePose = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlParaSimple.coordinateType = CoordinateType.baseCoordinate;
            pointControlParaSimple.pointTransType = PointTransType.pointTransStop;
            pointControlParaSimple.motiontriggerMode = MotiontriggerMode.MovetriggerbyOnlyRpc;

            CS.cr_move_pointControlPara_transfer(robotHandle, pointControlParaSimple, ref pointControlPara);
            CRresult result = CS.cr_move_line(robotHandle, pointControlPara, isBlock);
        }

        /// <summary>
        /// 移动X轴到指定点位
        /// </summary>
        /// <param name="number"></param>
        public void Movechange(double[] pose)
        {
      

            PointControlPara pointControlPara = new PointControlPara();
            PointControlParaSimple pointControlParaSimple = new PointControlParaSimple();

            bool isBlock = true;
            pointControlParaSimple.speed = new double[6] { 5, 5, 5, 5, 5, 5 };
            pointControlParaSimple.acc = new double[6] { 5, 5, 5, 5, 5, 5 };
            pointControlParaSimple.jointpos = basePose;
            //起始点示例数据
            pointControlParaSimple.pose = pose;
            //目标点示例数据
            pointControlParaSimple.tcpOffset = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlParaSimple.coordinatePose = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlParaSimple.coordinateType = CoordinateType.baseCoordinate;
            pointControlParaSimple.pointTransType = PointTransType.pointTransStop;
            pointControlParaSimple.motiontriggerMode = MotiontriggerMode.MovetriggerbyOnlyRpc;

            CS.cr_move_pointControlPara_transfer(robotHandle, pointControlParaSimple, ref pointControlPara);
            CRresult result = CS.cr_move_joint(robotHandle, pointControlPara, isBlock);
        }

        /// <summary>
        /// 点动模式
        /// </summary>
        /// <param name="number"></param>
        public void MoveToPoint(double x)
        {


            PointControlPara pointControlPara = new PointControlPara();
            PointControlParaSimple pointControlParaSimple = new PointControlParaSimple();

            bool isBlock = true;
            pointControlParaSimple.speed = new double[6] { 5, 5, 5, 5, 5, 5 };
            pointControlParaSimple.acc = new double[6] { 5, 5, 5, 5, 5, 5 };
            pointControlParaSimple.jointpos = basePose;
            pointControlPara.pose = new double[6] { 0, 5, 0, 0, 0, 0 };
            //沿坐标系 y 轴运动 5mm
            pointControlPara.jointpos = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlPara.tcpOffset = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlPara.coordinatePose = basepoint;
            //pointControlPara.coordinatePose = new double[6] { 114.3, -390.498, 215.347, -180, 0, 0 };
            //点坐标系
            pointControlPara.jerk = new double[6] { 60, 60, 60, 60, 60, 60 };
            pointControlPara.tcpID = -1;
            pointControlPara.coordinateType = CoordinateType.PointCoordinate;
            //基于点坐标系进行点动
            pointControlPara.pointTransType = PointTransType.pointTransStop;
            pointControlPara.pointTransRadius = 0;
            pointControlPara.poseTranType = PoseTranType.poseTranMoveToTargetPose;
            pointControlPara.motiontriggerMode = MotiontriggerMode.MovetriggerbyOnlyRpc;
            CRresult result = CS.cr_moveJog(robotHandle, pointControlPara);
            
        
        }

        public void Movejog_tcp(int value, double val, double speed = 2)
        {
            double acc = 10;
            double[] actualjointpos = new double[6];
            CS.cr_get_jointActualPos(robotHandle, actualjointpos);
            PointControlPara pointControlPara_movejog_tcp;
            //pointControlPara_movejog_tcp.jointpos = basePose;
       
            pointControlPara_movejog_tcp.jointpos = new double[6] { 0, 0, 0, 0, 0, 0 };
            for (int i = 0; i < actualjointpos.Length; i++)
            {
                pointControlPara_movejog_tcp.jointpos[i] = actualjointpos[i];
            }

            pointControlPara_movejog_tcp.speed = new double[6] { speed, speed, speed, speed, speed, speed };
            pointControlPara_movejog_tcp.acc = new double[6] { acc, acc, acc, acc, acc, acc };

            pointControlPara_movejog_tcp.pose = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlPara_movejog_tcp.tcpOffset = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlPara_movejog_tcp.coordinatePose = new double[6] { 0, 0, 0, 0, 0, 0 };
            //pointControlPara_movejog_tcp.coordinatePose = basepoint;
            
            pointControlPara_movejog_tcp.jerk = new double[6] { 0, 0, 0, 0, 0, 0 };
            pointControlPara_movejog_tcp.tcpID = -1;
            pointControlPara_movejog_tcp.coordinateType = CoordinateType.baseCoordinate;
            pointControlPara_movejog_tcp.pointTransType = PointTransType.pointTransStop;
            pointControlPara_movejog_tcp.pointTransRadius = 0;
            pointControlPara_movejog_tcp.poseTranType = PoseTranType.poseTranMoveToTargetPose;
            pointControlPara_movejog_tcp.motiontriggerMode = MotiontriggerMode.MovetriggerbyMoveCache;
            pointControlPara_movejog_tcp.pose[value] = val;
            //执行运行命令
            CRresult result = CS.cr_moveJog(robotHandle, pointControlPara_movejog_tcp);
            //循环检测运行结果
            Thread.Sleep(500);
            var robotMode = Get_robotMode();
            while (true)
            {
                robotMode = Get_robotMode();
                if (robotMode == RobotModes.ProgramStop)
                {
                    Thread.Sleep(300);
                    break;
                }
                Thread.Sleep(100);
            }
        }



        public RobotModes Get_robotMode()
        {
            RobotModes robotMode = RobotModes.BackDrive;
            CRresult result = CS.cr_get_robotMode(robotHandle, ref robotMode);
            return robotMode;
        }

        //读实际TCP位置
        public double[] api_demo_cr_get_tcpActualPose()
        {
            double[] pose = new double[6];
            CRresult result = CS.cr_get_tcpActualPose(robotHandle, pose);
            return pose;


        }

        //读目标TCP位置
        public double[] api_demo_cr_get_tcpTargetPose()
        {
            double[] pose = new double[6];
            CRresult result = CS.cr_get_tcpTargetPose(robotHandle, pose);
            return pose;
        }
        //读实际关节位置
        public double[] api_demo_cr_get_jointActualPos()
        {
            double[] actualpos = new double[6];
            CRresult result = CS.cr_get_jointActualPos(robotHandle, actualpos);
            return actualpos;
        }
    }
}
